%% Script to Process and Plot data from test with exo and different network architecture
clc
clear variables
close all

addpath("Functions\")

ChooseData = [1,2,3];
% 1 = exo only
% 2 = transparent
% 3 = 6G emulator

%% Data Open (do not modify)
pathData = '..\01 - Data\';
FileDataNames = {'ExoOnly';'TransparentNet';'6GEmulator'};
for dataIdx = 1:length(ChooseData)
    if ChooseData(dataIdx) == 1 % exo only
        filename_ML=[pathData,FileDataNames{ChooseData(dataIdx)},'.bin'];
        file_size =  38;
        fid = fopen(filename_ML,'r','s');
        dataML = fread(fid, [file_size inf],'double');
        fclose(fid);
        dataFPGA = [];
    else
        % FPGA (LL) File
        filename_FPGA=[pathData,FileDataNames{ChooseData(dataIdx)},'_LLC.bin'];
        file_size =  18;
        fid = fopen(filename_FPGA,'r','s');
        dataFPGA = fread(fid, [file_size inf],'double');
        fclose(fid);

        % ML File
        filename_ML=[pathData,FileDataNames{ChooseData(dataIdx)},'_MLC.bin'];
        file_size =  39;
        fid = fopen(filename_ML,'r','s');
        dataML  = fread(fid, [file_size inf],'double');
        fclose(fid);

        % histogram data FPGA
        filename_FPGA=[pathData,'2kDMA_',FileDataNames{ChooseData(dataIdx)},'_LLC.bin'];
        file_size = 5;
        fid = fopen(filename_FPGA,'r','s');
        HistDataFPGA = fread(fid, [file_size inf],'double');
        fclose(fid);

        % histogram data FPGA
        filename_FPGA=[pathData,'2kDMA_',FileDataNames{ChooseData(dataIdx)},'_MLC.bin'];
        file_size = 5;
        fid = fopen(filename_FPGA,'r','s');
        HistDataML = fread(fid, [file_size inf],'double');
        fclose(fid);

        [HistDataRaw{1,dataIdx}.LLCL,HistDataRaw{1,dataIdx}.MLCL] = HistDataLabeling(HistDataFPGA,HistDataML); 
    end
    [DataRaw{1,dataIdx}.LLCL,DataRaw{1,dataIdx}.MLCL] = DataLabeling(dataFPGA,dataML);
end

%% Show data
dataIdx = 1;
figure, 
ax(1) = subplot(311);
hold on;
plot(DataRaw{1,dataIdx}.MLCL.LeftJointAngle); plot(DataRaw{1,dataIdx}.MLCL.RightJointAngle);...
    plot(DataRaw{1,dataIdx}.MLCL.AssistON*20); plot(DataRaw{1,dataIdx}.MLCL.SynchON*20);...
    legend('LX Angle (deg)', 'RX Angle (deg)', 'AssistON', 'SynchON')
ax(2) = subplot(312);
plot(DataRaw{1,dataIdx}.MLCL.Phase/max(DataRaw{1,dataIdx}.MLCL.Phase)*100); legend('Gait Phase (%)')
ax(3) = subplot(313);
plot(DataRaw{1,dataIdx}.MLCL.LeftActualTorque); legend('LX Joint Torque (Nm)')
linkaxes(ax,'x')
hold off;

%% Preprocess
% indicate slow and fast steps to merge
StepMerge.Slow = 1:5;
StepMerge.Fast = 35:40;

DoDebugging = 1;
for dataIdx = 1:length(ChooseData)
    % Identify Assistance data
    Cut = find(DataRaw{1,dataIdx}.MLCL.AssistON==1);
    Data{1,dataIdx}.ActualTorque = DataRaw{1,dataIdx}.MLCL.LeftActualTorque(Cut);
    Data{1,dataIdx}.DesiredTorque = DataRaw{1,dataIdx}.MLCL.LeftDesiredTorque(Cut);
    Data{1,dataIdx}.Phase = DataRaw{1,dataIdx}.MLCL.Phase(Cut);
    Data{1,dataIdx}.JointAngle = DataRaw{1,dataIdx}.MLCL.LeftJointAngle(Cut);
    if ChooseData(dataIdx) > 1 % for histogram analysis
        CutFPGA = find(DataRaw{1,dataIdx}.LLCL.AssistON==1);
        Data{1,dataIdx}.IterMLCL = DataRaw{1,dataIdx}.MLCL.Iter(Cut);
        Data{1,dataIdx}.IterLLCL = DataRaw{1,dataIdx}.LLCL.Iter(CutFPGA);
    end

    % Identify Steps for segmentation
    A = [0, diff(Data{1,dataIdx}.Phase)];
    Data{1,dataIdx}.StepCut = find(A<-5)-1;

    % Debug Segmentation
    if DoDebugging 
        figure
        plot(Data{1,dataIdx}.Phase)
        hold on
        scatter(Data{1,dataIdx}.StepCut,Data{1,dataIdx}.Phase(Data{1,dataIdx}.StepCut))
        yyaxis right
        plot(Data{1,dataIdx}.DesiredTorque)
        plot(Data{1,dataIdx}.ActualTorque)
        scatter(Data{1,dataIdx}.StepCut,Data{1,dataIdx}.DesiredTorque(Data{1,dataIdx}.StepCut))
    end

    % Segment Steps
    for i = 2:length(Data{1,dataIdx}.StepCut)-1 % discard first and last step
        Steps{1,dataIdx}{i-1,1}.Phase = Data{1,dataIdx}.Phase(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
        Steps{1,dataIdx}{i-1,1}.ActualTorque = Data{1,dataIdx}.ActualTorque(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
        Steps{1,dataIdx}{i-1,1}.DesiredTorque = Data{1,dataIdx}.DesiredTorque(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
        Steps{1,dataIdx}{i-1,1}.JointAngle = Data{1,dataIdx}.JointAngle(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
        if ChooseData(dataIdx) > 1 % for histogram analysis
            Steps{1,dataIdx}{i-1,1}.IterMLCL = Data{1,dataIdx}.IterMLCL(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
            Steps{1,dataIdx}{i-1,1}.IterLLCL = Data{1,dataIdx}.IterLLCL(Data{1,dataIdx}.StepCut(i):Data{1,dataIdx}.StepCut(i+1));
        end
    end

    if DoDebugging
        % show the count duration for the steps you selected
        B = diff(Data{1,dataIdx}.StepCut);
        figure
        hold on
        plot(B)
        scatter(StepMerge.Slow,B(StepMerge.Slow),'r')
        scatter(StepMerge.Fast,B(StepMerge.Fast),'b')
    end

    % indices for histogram analysis
    % data from RT and FPGA can be synchronized by using the iteration
    % number
    if ChooseData(dataIdx) > 1
        HistIdx{1,dataIdx}.Slow.MLCL = [Steps{1,dataIdx}{StepMerge.Slow(1)}.IterMLCL(1),Steps{1,dataIdx}{StepMerge.Slow(end)}.IterMLCL(end)];
        HistIdx{1,dataIdx}.Slow.LLCL= [Steps{1,dataIdx}{StepMerge.Slow(1)}.IterLLCL(1),Steps{1,dataIdx}{StepMerge.Slow(end)}.IterLLCL(end)];
        HistIdx{1,dataIdx}.Fast.MLCL = [Steps{1,dataIdx}{StepMerge.Fast(1)}.IterMLCL(1),Steps{1,dataIdx}{StepMerge.Fast(end)}.IterMLCL(end)];
        HistIdx{1,dataIdx}.Fast.LLCL= [Steps{1,dataIdx}{StepMerge.Fast(1)}.IterLLCL(1),Steps{1,dataIdx}{StepMerge.Fast(end)}.IterLLCL(end)];
    end

    % Merge steps
    f = fieldnames(StepMerge);
    for velName = 1:length(f)
        for i = 1:length(StepMerge.(f{velName}))
            Length(i) = length(Steps{1,dataIdx}{StepMerge.(f{velName})(i),1}.JointAngle);
        end
%         MedLength = median(Length); % keep this to have plots in time domain
        MedLength = 1000; % keep this to have plots in phase domain
        ff = fieldnames(Steps{1,dataIdx}{1});
        for varName = 1:length(ff)
            for i = 1:length(StepMerge.(f{velName}))
                MergedSteps{1,dataIdx}.(f{velName}).(ff{varName})(i,:) = resampling(Steps{1,dataIdx}{StepMerge.(f{velName})(i)}.(ff{varName}),MedLength);
            end
            [MergedSteps{1,dataIdx}.(f{velName}).(['Std_',ff{varName}]),MergedSteps{1,dataIdx}.(f{velName}).(['Avg_',ff{varName}])]=std(MergedSteps{1,dataIdx}.(f{velName}).(ff{varName}));
        end
    end
    MergedSteps{1,dataIdx}.Slow.Variability_Percentage = mean(MergedSteps{1,dataIdx}.Slow.Std_ActualTorque)*100/14;
    MergedSteps{1,dataIdx}.Fast.Variability_Percentage = mean(MergedSteps{1,dataIdx}.Fast.Std_ActualTorque)*100/14;
end

%% Torque Tracking Plots
Colors = [1,0,0; ...
          0,0,1];
PhaseDomain = 1; % put 1 to have plot in phase domain, put 0 to have plot in time domain

Fig = figure;
Fig.Color='w';
for dataIdx = 1:length(ChooseData)
    subplot(length(ChooseData),1,dataIdx)
    hold on
    if PhaseDomain
        X = [1:1000]./10; % for results in phase domain
        StdShade_v2(MergedSteps{1,dataIdx}.Slow.Avg_ActualTorque,MergedSteps{1,dataIdx}.Slow.Std_ActualTorque,Colors(1,:),10);
    else
        X = [1:length(MergedSteps{1,dataIdx}.Slow.Avg_ActualTorque)]./100; % for results in time domain
        StdShade_v2(MergedSteps{1,dataIdx}.Slow.Avg_ActualTorque,MergedSteps{1,dataIdx}.Slow.Std_ActualTorque,Colors(1,:),100);
    end
    p(1) = plot(X,MergedSteps{1,dataIdx}.Slow.Avg_ActualTorque,'r','LineStyle','-');
    plot(X,MergedSteps{1,dataIdx}.Slow.Avg_DesiredTorque,'r','LineStyle','--');
    
    if PhaseDomain
        X = [1:1000]./10; % for results in phase domain
        StdShade_v2(MergedSteps{1,dataIdx}.Fast.Avg_ActualTorque,MergedSteps{1,dataIdx}.Fast.Std_ActualTorque,Colors(2,:),10);
    else
        X = [1:length(MergedSteps{1,dataIdx}.Fast.Avg_ActualTorque)]./100; % for results in time domain
        StdShade_v2(MergedSteps{1,dataIdx}.Fast.Avg_ActualTorque,MergedSteps{1,dataIdx}.Fast.Std_ActualTorque,Colors(2,:),100);
    end
    p(2) = plot(X,MergedSteps{1,dataIdx}.Fast.Avg_ActualTorque,'b','LineStyle','-');
    plot(X,MergedSteps{1,dataIdx}.Fast.Avg_DesiredTorque,'b','LineStyle','--');
    if PhaseDomain
        xlim([0 100])
        xlabel("Gait Phase (%)")
    else
        xlim([0 2])
        xlabel("Time (s)")
    end
    ylabel("Joint Torque (Nm)")    
    grid on
end
legend(p,["2.5 km/h","5 km/h",])

%% Histogram plots
DataHist = find(ChooseData > 1);
if isempty(DataHist)==0
    Fig = figure;
    Fig.Color='w';
    set(gcf, 'Units', 'Normalized', 'OuterPosition', [0.3, 0.3, 0.4, 0.5]);
    for histId = 1:length(DataHist)
        dataIdx = DataHist(histId);
        ax(1)=subplot(2,length(DataHist),histId);
        % pick the DMA interval corresponding to RT iterations
        % of the steps considered
        ML1 = find(HistDataRaw{1,dataIdx}.MLCL.RT_ItCnt == HistIdx{1,dataIdx}.Slow.MLCL(1),1);
        ML2 = find(HistDataRaw{1,dataIdx}.MLCL.RT_ItCnt == HistIdx{1,dataIdx}.Fast.MLCL(2),1);
        IdxMLAll = [ML1:ML2];

        hold on
        h(1)=histogram(HistDataRaw{1,dataIdx}.MLCL.EmulatedCounter_Diff(IdxMLAll),'BinWidth',1,'Normalization','probability','FaceColor','g');
        ylim([0 1.1]);
        ytix = get(gca, 'YTick');
        set(gca, 'YTick',ytix, 'YTickLabel',ytix*100)
        edges = h(1).BinEdges;
        values = h(1).Values;
        ind = find(round(values*100)>0);
        values = values(ind);
        centers = edges(1:end-1)+diff(edges)/4;
        centers = centers(ind);
        text(centers, values+0.05, string(round(values*100*10)/10), 'HorizontalAlignment', 'center','FontSize',10,'Color','k')
        xlim([-6,6]);
        ylabel('(%)');
        title('EmulatedCounterDiff MLCL')

        ax(2)=subplot(2,length(DataHist),histId+length(DataHist));
        % pick the DMA interval corresponding to RT iterations
        % of the steps considered
        ML1 = find(HistDataRaw{1,dataIdx}.LLCL.RT_ItCnt == HistIdx{1,dataIdx}.Slow.LLCL(1),1);
        ML2 = find(HistDataRaw{1,dataIdx}.LLCL.RT_ItCnt == HistIdx{1,dataIdx}.Fast.LLCL(2),1);
        IdxFPGAAll = [ML1:ML2];

        hold on
        h(1)=histogram(HistDataRaw{1,dataIdx}.LLCL.EmulatedCounter_Diff(IdxMLAll),'BinWidth',1,'Normalization','probability','FaceColor','g');
        ylim([0 1.1]);
        ytix = get(gca, 'YTick');
        set(gca, 'YTick',ytix, 'YTickLabel',ytix*100)
        edges = h(1).BinEdges;
        values = h(1).Values;
        ind = find(round(values*100)>0);
        values = values(ind);
        centers = edges(1:end-1)+diff(edges)/4;
        centers = centers(ind);
        text(centers, values+0.05, string(round(values*100*10)/10), 'HorizontalAlignment', 'center','FontSize',10,'Color','k')
        xlim([-6,6]);
        ylabel('(%)');
        linkaxes(ax,'x')
        title('EmulatedCounterDiff LLCL');
    end
end